#include "WrmnkARXWrapper.h"

#include <algorithm>
#include <numeric>

#include <iostream>

// Przestrzeń nazw
namespace RafMat
{
namespace SimEngine
{

using namespace std;

// Do debugowania:
//static void printVect1(ostream& o, const std::string n, const RmnkSolver::Matrix& m)
//{
//    o << n << ":[ ";
//    RmnkSolver::Matrix::const_iterator1 it;
//    for(it =m.begin1(); it != m.end1(); ++it)
//        o << *it << ' ';
//    o << "]" << endl;
//}

WrmnkARXWrapper::WrmnkARXWrapper(unsigned dA, unsigned dB, unsigned dly, double lambda)
    : m_dA(dA), m_dB(dB), m_rankSum(dA + dB + 1), m_delay(dly), m_lambda(lambda)
{
    this->reset();
}

void WrmnkARXWrapper::reset()
{
    m_wrmnk.reset(new RmnkSolver(m_dA + m_dB + 1, m_lambda));

    m_estimatedA = Polynomial(m_dA, 0);
    m_estimatedA[0] = 1; // Współczynnik a0
    m_estimatedB = Polynomial(m_dB, 0);

    // Pamięć próbek wyjściowych obejmuje próbki: y(i-1)...y*(i-dA)
    m_outmem.resize(m_dA);

    // Pamięć próbek wejściowych obejmują próbki: u(i-n)...u(i-n-dB)
    // trzeba więc pamiętać n+dB+1 wartości (włącznie z u(i)...u(i-n-dB).
    m_inmem.resize(m_dB + m_delay + 1);

    // INICJALIZACJA MACIERZY
    // 1. Wektor szukanych parametrów = zlepek współczynników A i B.
    m_theta = Matrix(m_rankSum, 1);

    // 2. Wektor wejść/wyjść, każdy z elementów odpowiada jakiemuś współczynnikowi
    // licznika bądź mianownika transmitancji, czyli wymiar to także dA+dB.
    m_varphi = Matrix(m_rankSum, 1);

    //printVect1(std::cout, "theta(0) = ", m_theta);
    //printVect1(std::cout, "phi(0) = ", m_varphi);
}

void WrmnkARXWrapper::identify(double inSample, double outSample)
{
    m_inmem.push_front(inSample);
    m_inmem.pop_back();

    // Zawartość m_inmem: u(i) ... u(i-k-dB)
    // Zawartość m_outmem: y(i-1) ... y(i-dA)

    Matrix::iterator1 bnd_it = m_varphi.begin1() + m_dB + 1; // granica między u i -y.

    // Kopiuj próbki u(i-k) .. u(i-k-dB)
    copy(m_inmem.begin() + m_delay, m_inmem.end(), m_varphi.begin1());

    // Kopiuj próbki y(i-1) .. y(i-dA)
    copy(m_outmem.begin(), m_outmem.end(), bnd_it);

    // Zmień znaki y
    transform(bnd_it, m_varphi.end1(), bnd_it, negate<double>());

    // Dokonaj nowej oceny parametrów modelu ARX.
    m_theta = m_wrmnk->identify(m_theta, m_varphi, outSample);

    //printVect1(std::cout, "theta = ", m_theta);

    // Konwersja do obiektów klasy Polynomial
    copy(m_theta.begin1(), m_theta.begin1() + m_dB + 1, m_estimatedB.begin());
    copy(m_theta.begin1() + m_dB + 1, m_theta.end1(), m_estimatedA.begin()+1);

    // Zapamiętujemy próbkę wyjściową
    m_outmem.push_front(outSample);
    m_outmem.pop_back();
}

}
}
